#include <ctime>
#include <cmath>
#include"OutputHandler.cpp"

class VirtualCoordinateProcessor
{
private:
	int targetX;
	int targetY;
	long virtualx;
	long virtualy;
	time_t lasttime;
	//int lasttime;
	float PWMX;
	float PWMY;
	float vAngle;
	float hAngle;
	int previousDistance;
	OutputHandler* MCH;


public:
	VirtualCoordinateProcessor(OutputHandler* temp)
	{
		//send initailazation coordinates
		time(&lasttime);
		hAngle = 60;
		vAngle = 60;
		virtualx = 90;
		virtualy = 90;
		PWMX = 1.5;
		PWMY = 1.5;
		//lasttime = clock();
		previousDistance = 0;
		MCH = temp;
	}
	bool set_Target(int x,int y)
	{
		float sendX;
		float sendY;
		float speed;
		float diff;
		long varianceX;
		long varianceY;
		time_t currentTime;
		int newdistancex;
		int newdistancey;
		int speedx;
		int speedy;
		int tempx;
		int tempy;
		//int currentTime;

		//send coordinates---using a basic linear mapping
		sendX = (hAngle/800) * x; //this will need some calibration to find the limits of the cammera
		sendY = (vAngle/600) * y; //same here

		PWMX = (1/180)*sendX + 1;
		PWMY = (1/180)*sendY + 1;

		//send the coordinates using the output handler
		MCH->SendPWM(PWMX,PWMY);
		time(&currentTime);
		diff = difftime(currentTime,lasttime);
		newdistancex = targetX - ((800/hAngle)*virtualx);
		newdistancey = targetY - ((600/vAngle)*virtualy);
		
		if(newdistancex < 0)
		{
			speedx = -333;

			if((speedx * diff) <= newdistancex)
			{
				// target reached
				virtualx = ((800/hAngle)*virtualx);
			}
			else
			{
				//target not reached
				virtualx = (speedx * diff);
			}
		}
		else
		{
			speedx = 333;
			if((speedx * diff) <= newdistancex)
			{
				//target not reached
				virtualx = (speedx * diff);
			}
			else
			{
				// target reached
				virtualx = ((800/hAngle)*virtualx);
			}
		}

		if(newdistancey < 0)
		{
			speedy = -333;

			if((speedy * diff) <= newdistancey)
			{
				// target reached
				virtualy = ((800/vAngle)*virtualy);
			}
			else
			{
				//target not reached
				virtualy = (speedy * diff);
			}
		}
		else
		{
			speedy = 333;
			if((speedy * diff) <= newdistancey)
			{
				//target not reached
				virtualy = (speedy * diff);
			}
			else
			{
				// target reached
				targetY = ((800/vAngle)*virtualy);
			}
		}

		
		if((abs(targetX - virtualx) + abs(targetY - virtualy)) < 100)
		{
			return true;
		}
		else
		{
			return false;
		}

	}
};

/*int main()
{
	return 0;
}*/
